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orientation  angle  control  law  to  ensure  stability  and 
accuracy.  Sensitivity  analysis  with  respect  to  inaccuracies  in 
the  knowledge  of  the  vehicle  hydrodynamic  characteristics  is 
performed.  The  results  demonstrate  the  validity  of  this 
approach  and  offer  a  way  to  achieve  accurate  path  keeping  in 
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I .   INTRODUCTION 

One  of  the  most  important  functions  of  a  marine  vehicle  is 
accurate  path  keeping  through  prescribed  routes.  Path  accuracy 
is  particularly  important  when  the  vehicle  has  to  operate  in 
confined  waters.  In  the  case  of  an  unmanned  vehicle,  the 
commanded  path  is  usually  approximated  by  straight  line 
segments  between  consecutive  way  points.  This  assumption  works 
well  if  the  desired  route  is  a  smooth  path  and  can  be 
approximated  by  a  series  of  straight  line  segments.  This  has 
been  studied  extensively  in  the  past.  Lienard  [1]  designed  a 
steering  sliding  mode  control  autopilot  based  on  the  line  of 
sight,  while  Chism  [2]  studied  a  cross  track  error  based 
control  law.  Hawkinson  [3]  extended  the  results  to  the 
multiple  input  case  when  bow  and  stern  planes  are 
independently  controlled.  Instead  of  using  the  cross  track 
error  directly  in  the  control  law,  similar  path  keeping 
characteristics  can  be  achieved  by  using  a  line  of  sight 
pursuit  guidance  scheme.  This  is  accomplished  by  moving  the 
vehicle  in  the  direction  of  a  moving  way  point  which  is 
located  on  the  commanded  straight  line  path  [4].  Stability  of 
this  scheme  depends  on  the  control  law  gains  and  the  look 
ahead  distance  between  the  vehicle  and  the  moving  way  point 
[5],  The  technique  can  be  applied  to  combined  steering  and 


diving  response  [6].  In  this  work,  we  extend  the  previous 
studies  of  pursuit  guidance  in  the  case  of  curved  reference 
paths  with  piecewise  constant  curvature.  This  means  that  we 
adopt  a  purely  geometrical  construction  of  the  reference  path 
which  is  composed  of  a  series  of  straight  line  segments  and 
circular  arcs.  In  Chapter  II  we  present  an  overview  of  the 
equations  of  motion  in  the  horizontal  plane,  the  control  and 
the  guidance  laws.  We  use  the  full  set  of  maneuvering 
equations  for  simulation  and  a  reduced  model,  Nomoto's  first 
order  model,  for  steering  control  design.  This  is  because  the 
vehicle  parameters  in  the  reduced  order  model  can  be  estimated 
relatively  accurately  from  first  principles  and  experimental 
data  [7] .  In  Chapter  III  the  focus  is  on  stability.  We  present 
two  stability  conditions,  one  based  on  the  reduced  model,  and 
one  based  on  the  complete  vehicle  dynamics  model.  The 
sensitivity  of  the  stability  curves  with  respect  to  the 
vehicle  hydrodynamic  coefficients  and  control  parameters  is 
also  studied  in  this  chapter.  In  Chapter  IV  we  present  the 
development  and  simulation  results  of  the  guidance  scheme  for 
circular  reference  paths.  All  computations  in  this  work  are 
performed  for  the  NPS  AUV  II  vehicle  for  which  a  complete  set 
of  hydrodynamic  coefficients  and  geometric  properties  is 
available  [8]  .  Finally,  we  summarize  the  conclusions  of  this 
work  and  some  recommendations  for  further  research. 


II.   MATHEMATICAL  FORMULATION 

In  this  section  the  vehicle  equations  of  motion  for  the 
horizontal  plane  (x,y),  the  control  law  and  the  guidance 
scheme  are  presented. 

A.   EQUATIONS  OF  MOTION 

For  the  horizontal  plane  the  mathematical  model  consists 
of  the  following  simplified  expressions  for  the  nonlinear  sway 
and  yaw  differential  equations  shown  below  : 

m(v+ur+xGt)=Y  (2.1) 

Iz?+mxG(v+ur)=N  (2.2) 

Equations  (2.1),  (2.2)  were  derived  from  the  general  six 
degrees  of  freedom  equations  for  a  vehicle  by  setting  all 
terms  related  to  the  vertical  plane  motion  to  be  zero, 
assuming  port/starboard  symmetry.  The  equations  for  the  sway 
force  Y  and  yaw  moment  N  are  : 

Y-Yzt+  ( Y+V+Yzlir)  +Yvliv+Ybu26 
N=NTt+  (N^v+Nzur)  +Nvuv+N6u2{> 


Finally  the  three  kinematic  equations  of  the  inertial  position 
rates   and  yaw  rate  are  : 

ijr=r  (2.3) 

x=ucos\|r-vsini|f  (2.4) 

y=usini|r  +  vcosT|f  (2.5) 

B.       CONTROL   LAW 

The  coupled  equations  (2.1)  and  (2.2)  after  some  algebra 
appear  as  shown  : 

v'=a11uv+a12ur+jb1u26  (2.6) 

?=a21uv+a22ur+b2u26  (2.7) 

where   the   coefficients   au,    a12,    a21,    a22,    b2   and  b2   are    : 

D=(IZ-NZ)  (m-Y+)-(mxG-Yz)  (mxG-Nj 


an  =  J5  I  dz-Xt)  Yv-(mxG-Yr)Nv] 


ai2="5  [  (Jz-^)  l-m+Yz)  -imXg-Yj  {-mxG+Nz)  ] 


a2i  =  "^  (m-Y+)Nv-  (mxG-N+)  Yv] 


a22  =  ^  t  im-Y+)  (-mxG+Nz)  -  {mxG-Nj  (-m+Yr)  ] 


V^  I  ( Iz~Nt)  V  (mxG-Yt)  Nbm-  (Iz-Nt)  Yt^  (mxG-Yt)  tf,J 
b2  =  ±  [  im-Yj  N6f-  (mx0-Ni)  Yhm-  (m-Y+)  N6b+  (mxG-N+)  Y^] 

Nondimensionilizing  the  above  equations  in  terms  of  the  water 
density  p  the  vehicle  length  L  and  the  nominal  forward  speed 
U  of  the  vehicle,  results  in  the  following  : 

^a^v+a^r+b^  (2.8) 

t=a21v+a22r+b26  (2.9) 

From  equations  (2.8)  and  (2.9),  the  relationship  of  the 
vehicle  turning  rate  r  and  the  rudder  angle  8  appears  through 
a  second  order  transfer  function  : 

r  jb,s+a,,.ibi  -a,  ,2x, 

(2.10) 


6     s2-  (van)  s+axla22-a12a21 

Performing  a  Taylor  series  expansion  in  the  inverse  of 
equation  (2.10)  and  keeping  the  first  two  terms  we  arrive  at 
a  simplified  first  order  transfer  function  : 

4=       °2  (2.11) 

o  cx+c2s 


where 


c±=-(alxa22-a2xaX2)  {a^b^  axlb2) 

C2=(ail+a22>    («aA-«lA)  +2>2<aiia22-a2iai2) 

c3=-(a2A-aiA)2 

Equation  (2.11)  can  be  also  written  as  : 

?=ar+bb 


where 


a=— El        and  b=-^- 

C2  C2 


In  this  way,  the  vehicle  turning  dynamics  reduce  to  the 
approximate  system  of  the  following  two  equations  : 

ijr=r  (2.12) 

r=ar+bb  (2.13) 

The  control  law  then  will  be  based  on  y  and  r  and  this  is 
going  to  be  in  the  form  : 

b=k^+k2z  (2.14) 

Solving  for  \\f   from  equations  (2.12),  (2.13)  and  (2.14)  we  end 
up  with  a  second  order  differential  equation  as  below  : 


ty  -  ( a  +bk2 )  i|r  -bk^ =0 

In  terms  of  the  desired  natural  frequency  0)n  and  the  damping 
ratio  £  the  gains  kx  and  k2  are  given  by  the  following 
relations    : 

2 


^.-i.  (2.15) 


£ 


JaI2^£  (216) 

2  £> 


Therefore,  the  feedback  gains  for  the  control  law  can  be 
determined  based  on  the  desired  values  of  G)n  and  £. 

C .   GUIDANCE 

The  heading  autopilot  that  was  designed  through  the 
control  law  in  the  previous  section  is  called  upon  now  to 
provide  vehicle  path  keeping  through  a  series  of  way  points  in 
the  horizontal  plane  and  as  we  will  see  later  accurate  path 
keeping  along  circular  paths.  In  order  to  achieve  it  .and  at 
the  same  time  to  keep  the  same  heading  autopilot  design  we 
have  to  use  a  suitable  navigation  scheme  such  as  the  line  of 
sight  guidance  scheme  (or  the  look  ahead  distance  scheme) .  In 
the  above  scheme  the  autopilot  attempts  to  position  the 


longitudinal  axis  of  the  vehicle  towards  a  point  D  which  is 
located  ahead  of  the  vehicle's  nominal  path  at  a  fixed 
distance  d  as  shown  in  Figure  1.  This  fixed  distance  d  is  the 
look  ahead  distance  and  the  line  of  sight  angle  o  is  defined 
as  : 

tano  =  -^  (2.17) 

d 

For  pure  pursuit  navigation  the  corresponding  control  law  by 
modifying  equation  (2.14)  becomes  : 

where  : 

Tcom" 

By  introducing  equation  (2.17)  in  the  control  law  equation, 
the  commanded  vehicle  heading  angle  is  not  constant  any  more 
but  a  function  of  the  vehicle's  position  y  as  it  can  be  seen 
from  equation  (2.5).  Since  the  control  law  was  based  on 
equations  (2.3),  (2.6)  and  (2.7)  without  including  equation 
(2.5)  for  lateral  displacement,  the  selected  gains  kx,  k2  do 
not  guarantee  stability  any  more  and  therefore  conditions  must 
be  developed  to  guarantee  stability  and  ensure  satisfactory 
path  keeping. 
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Figure  1.  Geometry  and  Definitions  of  Symbols 


III.   STABILITY 

A.   APPROXIMATE  CONDITION  FOR  STABILITY 

So  far  the  complete  system  is  given  by  the  approximate 
equations  (2.12),  (2.13),  and  the  inertial  position  rate 
equation  (2.5).  In  its  linearized  and  dimensionless  form  for 
a  very  small  lateral  velocity  v  this  becomes  : 

y=i|r  (3.1) 


The  control  law  including  the  guidance  scheme  is 


6  =^3^  ( ijr  +tan~l  -^ )  +k2r 


which  in  its  linearized  form  is  given  by  : 


6=kx(\\?  +  -£)+k2r  (3.2) 


Solving  equations  (2.12),  (2.13),  (3.1)  and  (3.2)  for  y  we  end 
up  to  with  a  third  order  differential  equation  as  follows  : 

bk 
y-(a+bk2)?-bk1?-—±=0  (3.3) 
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Writing  the  above  equation  in  the  s-domain  and  substituting  kj 
and  k2  from  equations  (2.15)  and  (2.16),  equation  (3.3)  takes 
the  following  form  : 

s3+2Co>ns2+coJs+-^=0  <3-4> 


The  Routh  criterion  for  stability,  BC-AD  >  0,  where  A,  B,  C 
and  D  are  the  coefficients  of  equation  (3.4),  applied  to  the 
above  equation  gives  the  following  condition  that  must  be 
satisfied  : 

d>—^—  (3.5) 

If  we  have  selected  a  natural  frequency  G)n  and  a  damping  ratio 
£  for  the  controller,  it  is  possible  from  the  above  relation 
to  have  a  quick  estimate  for  the  minimum  look  ahead  distance 
d  such  that  the  system  will  be  stable.  In  Figure  2  the 
approximate  stability  curve  (dotted  line)  was  plotted,  the 
look  ahead  distance  d  versus  the  natural  frequency  of  the 
controller  con,  based  on  equation  (3.5)  and  for  damping  ratio 
£=0.8.  This  is  in  general  considered  to  be  good  for  a  system, 
neither  too  sluggish,  nor  too  stiff.  As  is  discussed  in  the 
next  section  these  values  for  the  natural  frequency  and  the 
damping  ratio  give  also  reasonable  values  for  the  gains  kx  and 

JC2  • 


11 


3.5 


3  - 


2.5  - 


2  - 


1.5- 


_ J                 _                 '    j           ■ 1 

1  \                                                                                                          ! 

*    \ 

\     \     i                                                                                                                                             : 
\      \    i                                                                                                                                             : 
*      \i                                                                                     i 

i — i 

-    ■  ■— r 

i 

i      \                               i 
i   i    \                                                                        i 

\  ;    \                                                     :                              ; 
x !      \                        1 
i        \ 

i     \          \                                            !                                                                                                                             1 
!     x           \                                                                                                                                                             • 

\           \                                  ; 
1            \               \   :                                                                   1                                                                                                     i 

\                       \ 

i    \                !                                                  !                         i 

\           X          !                  1                  i 

I                   \               \                                   j                         !                         |                         i 

0.2 


0.3 


0.4 


0.5 
d 


0.6 


0.7 


0.8 


0.9 


Figure  2 .  Exact  and  Approximate  Stability  Curves  for 
Various  0^. 
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B.   EXACT  CONDITION  FOR  STABILITY 

For  an  exact  calculation  we  use  equations  (2.3),  (2.8), 
(2.9)  and  equation  (2.5)  which  in  linearized  form  appears  as: 

y=i|r  +  v  (3.6) 

This  set  completes  the  control  law  while  the  guidance  scheme 
is  given  equation  (3.2) .  Substituting  the  control  law  into  the 
equations  (2.8),  (2.9)  and  rearranging  the  terms  the  system 
becomes  as  shown  : 

ijf=r  (3.7) 

k  b 
v=k1b1^+a11v+  {a12+k2bx)  r+^±y  (3.8) 


t=k^b2y+a2xv+  (a22+k2b2)  r+^-y  (3.  9) 


y=\|/  +  v  (3.10) 

Local  stability  properties  are  established  by  the  eigenvalues 
of  the  stiffness  matrix.  The  characteristic  equation  of  this 
system  is  a  fourth  order  polynomial  in  the  form  : 

A\*+B\2+Ckz+Dk+E=0  (3.11) 


where 
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c=c^c2 


D=D1+D2 


with  A,    B,    Clr    C2,    Dlf    D2   and  E   defined  as    in  the    following 

B=-(a11+a22)-k2b2 

Cl=  (aiia22-ai2a2l)  +  («lA-*aA)  *2_icA 


C2"     d~ 


^r(aui)2-a2A)^ 


n*  d  "  d 


E_   (aiA-a2A)^i 


Routh's  criterion   for  stability  determines  that  loss  of 
stability  occurs  when  the  following  relation  is  satisfied  : 

BCD-B2E-AD2=0 

After  some  algebra  a  quadratic  equation  for  d  occurs  as  below: 

a1d2+a2d+a3=0  (3.12) 
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where  the  coefficients  0^,  a2,  oc3  are  defined  as  follows  : 

di=BCxDx-Dl 
a2=BC1D2d+BC2dD1-B2Ed-2  .  OD^d 

a3=BC2dD2d-D2d2 

The  positive  root  of  equation  (3.12)  determines  the  critical 
value  of  the  look  ahead  distance  d  for  stability.  For  every  d 
>  dcritical  the  system  is  stable  and  the  vehicle  will  follow  the 
desired  path.  In  the  other  case  when  d  <  dcrltlcal  the  system 
becomes  unstable  and  the  vehicle  moves  with  an  oscillatory 
motion  as  a  result  of  a  complex  conjugate  pair  of  eigenvalues 
with  positive  real  parts  that  appears  in  this  case.  In  Figure 
2  the  continuous  line  represents  the  exact  stability  curve  for 
various  values  of  the  G)n  versus  the  look  ahead  distance  d  and 
for  a  damping  coefficient  £=0.8.  As  can  be  seen  the 
approximate  stability  curve  (dotted  line)  is  very  close  to  the 
exact  and  therefore  it  can  be  used  as  a  good  first 
approximation  in  order  to  provide  adequate  stability  values  to 
the  system.  In  Figure  3  a  family  of  curves  was  plotted  for 
damping  ratios  with  values  of  1.2  to  0.4  with  step  0.1  (left 
to  right)  for  various  values  of  0^  versus  the  look  ahead 
distance  d.  As  can  be  seen  the  stability  area  is  increasing 
for  higher  values  of  the  natural  frequency  con  and  for  higher 
values  of  the  damping  ratio  £.  When  the  natural  frequency 
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Figure  3 .  Stability  Curves  for  Various  Damping  Ratios 
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though  reaches  a  value  of  3.0  or  higher,  it  is  observed  that 
the  stability  curves  for  the  various  damping  ratios  are 
approaching  each  other  giving  almost  the  same  stability  area. 
From  Figure  4  it  is  also  observed  that  for  a  natural  frequency 
of  3.0  the  gains  klr  k2  are  about  3.2  and  1.2  respectively 
while  higher  values  of  CO,,  give  higher  gains  without  much 
increase  in  the  stability  area.  Therefore  the  above  selection 
of  0^  and  t,  to  be  3.0  and  0.8  respectively,  is  considered 
good.  An  other  way  to  look  at  the  gains  is  that  for  1  degree 
of  heading  error  a  rudder  of  3.2  degrees  is  required  while  for 
1  degree/sec  turning  rate  a  1.2  rudder  angle  is  required. 

C.   SENSITIVITY  OF  STABILITY  CURVES 

The  hydrodynamic  coefficients  of  the  vehicle  on  which  all 
the  calculations  were  based  have  been  determined 
experimentally.  Therefore  it  is  realistic  to  assume  that  a 
degree  of  error  (uncertainty)  that  enters  the  calculations  and 
may  influence  the  results.  In  order  to  see  how  much  this 
uncertainty  for  each  coefficient  changes  the  stability  curves, 
a  variation  of  50%  of  the  nominal  value  for  each  hydrodynamic 
coefficient  was  assumed  and  plotted  versus  the  normalized 
look  ahead  distance  dn  (dnew/dnom)  .  The  coefficients  that  found 
to  affect  stability  were:  Y<>,  Yv,  Nt  and  Nr  while  the  rest  of 
the  coefficients  had  no  effect.  In  Figure  5  the  variation  of 
the  coefficient  Y^  was  plotted  versus  the  normalized  look 
ahead  distance  dn,  and  as  it  can  be  observed  a  variation  of  a 
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Figure  4.  Control  Feedback  Gains  versus  co^. 
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Figure  5.  Changes  of  Critical  Look  Ahead  Distance  d  for 
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50%  of  the  nominal  value  gives  a  total  change  of  about  0.40 
for  dn.  In  Figure  6  the  variation  of  Yv  versus  dn  was  plotted 
and  for  a  variation  50%  of  Yv  a  total  change  of  about  0.37  of 
dn  occurred.  In  Figure  7  the  variation  for  Nf  gives  a  total 
change  of  0.14  for  dn  while  in  Figure  8  the  variation  for  Nr 
gives  a  total  change  of  0.102  for  dn.  As  can  be  seen  the 
variation  of  Y^  and  Yv  gives  values  for  dn  of  about  four  (4) 
times  larger  than  Ut  and  Nr,  therefore  these  two  coefficients 
affect  the  look  ahead  distance  the  most.  For  all  the  above 
plots  above  the  values  for  natural  frequency  and  damping  ratio 
were  3.0  and  0.8  respectively.  In  an  attempt  to  see  how  much 
a  variation  of  C,  and  (0n  affects  the  previous  results  for  Y<,  and 
Yv  the  following  Figures  9,  10,  11  and  12  are  plotted.  It  can 
be  seen  that  the  maximum  deviation  in  the  critical  value  of  d 
for  stability  is  40%  of  its  nominal  value,  while  for  most  of 
the  cases  this  deviation  is  well  below  20%. 
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Figure  9.  Sensitivity  of  Critical  Look  Ahead  Distance  for 
Variation  in  Y*  and  for  Different  Damping  Ratios. 
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Figure  12 .  Sensitivity  of  Critical  Look  Ahead  Distance  for 
Variation  in  Yv  and  for  Different  Natural 
Frequencies . 
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IV.   SHIP  PATH  KEEPING 

After  satisfying  the  requirements  for  stability  by 
selecting  proper  values  for  the  gains  kx,  k2  and  the  look  ahead 
distance  d  as  was  discussed  in  the  previous  section,  the  ship 
path  keeping  problem  along  straight  line  segments  and  circular 
trajectories  is  examined  in  this  section. 

A.   STRAIGHT  LINE  SEGMENTS  PATH  KEEPING 

So  far  the  control  law  with  the  guidance  scheme  is  given 
by  equation  (2.18)  which  in  steady  state  gives  a  heading  angle 
of  zero  (0) .  In  order  for  the  vehicle  to  follow  a  straight 
line  path  between  two  given  points  (xlf  y:)  and  (x2/  y2)  the 
control  law  must  be  modified.  If  a,  is  the  angle  between  the 
straight  line  (P)  and  the  x  axis;  the  distance  y  that  enters 
the  control  law  becomes  y',  as  can  be  seen  from  Figure  13.  The 
modified  control  law  is  shown  below  : 

fi^sindjf-o-a)  +k2r  (4.1) 

where  (J  and  y '  are  given  by  : 


o=ycom=-arctan(-£) 


y-  (y-yi)  cosa-  (x-xx)  sina 
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Figure  13.  Guidance  Scheme  for  Straight  Line  Path 
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At  steady  state  now  the  heading  angle  becomes  a  and  the 
vehicle  follows  the  straight  line  (P) .  The  simulation  program 
takes  into  account  all  the  possible  different  orientations  of 
(P)  by  modifying  the  angle  a  properly  for  each  case.  In  Figure 
14  we  can  see  the  simulated  path  of  the  vehicle  (continuous 
line)  and  the  reference  path  (dotted)  for  a  two  way  point 
path.  The  reference  line  is  a  quarter  of  a  circle  while  the 
two  points  have  coordinates  (0,0)  and  (10,10)  respectively.  In 
Figure  15  the  rudder  time  history  for  the  above  two  way  point 
path  keeping  for  a  straight  line  is  presented.  The  maximum 
value  is  about  -23  degrees,  which  is  a  saturation  point  since 
the  rudder  is  not  allowed  to  exceed  this  value  (exact  0.4 
radian  or  22.923  degrees).  In  the  following  Figures  16 
through  23  the  number  of  points  was  increased  to  3,  5,  7  and 
13,  in  an  effort  to  approach  the  reference  path,  by  dividing 
the  reference  path  in  equally  spaced  arcs  and  following  the 
straight  line  segments  defined  by  these  points.  As  can  be 
seen,  by  increasing  the  number  of  points  the  vehicle  can 
follow  the  reference  path  with  increased  accuracy.  The 
drawbacks  of  simply  increasing  the  number  of  discretization 
points  are  two.  First,  we  are  placing  increased  demands  on 
storage  and  memory  for  the  on-board  processor.  Second,  even 
when  satisfactory  path  accuracy  is  achieved  by  using  a  large 
number  of  way  points  the  rudder  activity  is  very  excessive  and 
erratic.  Therefore  if  the  vehicle  must  follow  a  circular  arc 
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Figure  14.  Simulation  for  Reference  end  Vehicle  Path 
for  2  Way  Points. 
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Figure   15.    Rudder  Time   History   for  2  Way  Point  Path. 
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Figure  16.  Simulation  for  Reference  and  Vehicle  Path  for 
3  Way  Points. 
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Figure  17.  Rudder  Time  History  for  3  Way  Point  Path. 
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Figure  18.  Simulation  for  Reference  and  Vehicle  Path  for 
5  Way  Points . 
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Figure  19.  Rudder  Time  History  for  5  Way  Point  Path. 
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Figure  20.  Simulation  for  Reference  and  Vehicle  Path  for 
7  Way  Points . 
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Figure  21.  Rudder  Time  History  for  7  Way  Point  Path. 
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Figure  22.  Simulation  for  Reference  and  Vehicle  Path  for 
13  Way  Points. 
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Figure  23.  Rudder  Time  History  for  13  Way  Point  Path. 
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path,  doing  so  by  following  small  straight  line  segments  is 
not  the  best  way  since  a  large  number  of  points  is  needed  and 
the  rudder  is  doing  a  considerable  amount  of  effort.  This 
means  that  a  different  way  must  be  developed  such  that  the 
vehicle  at  any  time  t  is  following  the  circular  arc  instead  of 
a  straight  line. 

B.   CIRCULAR  ARC  PATH  KEEPING 

By  demanding  the  vehicle  to  follow  a  certain  circular  path 
in  a  certain  way,  clockwise  or  counterclockwise,  the  angle  G 
that  enters  the  control  law  is  going  to  vary  depending  on  the 
orientation  of  the  vehicle  relative  to  the  center  of  the  path. 
It  needs  to  change  values  from  positive  to  negative  in  a 
consistent  way  with  the  control  law  requirements,  in  order  for 
the  vehicle  to  negotiate  the  assigned  path.  For  this  reason  we 
need,  to  properly  modify  the  control  law  for  each  case  of  the 
vehicle  motion  (cw,  ccw)  and  for  each  quarter  of  the  circle 
separately.  Defining  by  A  the  distance  from  point  V  to  point 
0,  Figure  24,  A  is  given  by  the  following  expression  : 


A=[(x-x0)2+(y-y0)2]  2  (4'2) 
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Figure  24.  Guidance  Scheme  for  CW  Motion. 
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The  radius  of  the  circular  path  R  can  also  be  expressed  as  : 


^[(^-^oJMyx-yo)2]  2  (4,3) 


From  similar  triangles  the  following  relations  yield  the 
coordinate  values  of  point  3  : 

x3=x0  +  -|  (x-x0)  (4.4) 


y3=yo+^(y-y0)  <4-5> 


Equations  (4.2),  (4.3),  (4.4)  and  (4.5)  are  valid  for  CW  or 
CCW  motion  and  for  any  possible  orientation  of  the  vehicle. 
The  equations  that  are  valid  for  each  separate  case  are 
presented  below. 

1.   CLOCKWISE  MOTION 

a)  For  the  fourth  quarter  of  the  circle  and  from  Figure  24 
the  angle  63  can  be  written  as  : 


e3=arcsin(^_^l)  (4.6) 

3  R 


The  angle  62  also  appears  as  follows  : 
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62=71-0^63  (4.7) 


where  Q1   can  be  written   as 


8,-1-1(1)  (4-8. 


For  this  case  d  is  the  length  of  the  arc  from  point  3  to  point 
4,  while  d34  is  the  distance  between  points  3  and  4  and  can  be 
written  as  follows  : 


d_ 
2R 


d34=2i?sin(-^)  (4.9) 


The    coordinates    of   point    4    now    since    d34,    x3    and   y3    are    known 
can   be   evaluated   from  trigonometry    as    follows    : 


x4=x3+d34sin02  (4.10) 


y4=y3 +d34cos02  (4.11) 


The  distance  B  from  point  V  to  point  4  is 
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1 
S=[(x-x4)2+(y-y4)2] 


2,i>  (4.12) 


Defining  the  angle  y   as  following  : 

y=a+Q1  (4.13) 


and  performing  the  law  of  sines  in  the  triangle  (0V4),  y    is 
given  by  : 

Y=arcsin[sin(-^)4]  (4.14) 

R      B 


Substituting  angle  y,  equation  (4.13)  gives  angle  a.  For  the 
control  law  the  angle  a  (the  angle  that  d34  makes  with  the 
horizontal)  is  also  needed  and  this  is  given  by  : 


a  =  -?--60  (4.15) 


2   2 


Finally  the  control  law  takes  the  following  form  : 

6=kxsin{ty-a+a)  +k2r  (4.16) 

Taking  the  sine  of  the  total  heading  angle  accounts  for  the 
approximations  of  linearization  that  were  made  in  selecting 
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the  gains  klf  k2 .  The  above  form  of  the  control  law  is  valid 
for  all  of  the  quarters  of  the  clockwise  motion,  while  for  the 
counterclockwise  motion,  angle  G  is  reversing  sign  as  we  will 
see  later.  The  equations  (4.7),  (4.8),  (4.9),  (4.12),  (4.13) 
and  (4.14)  are  also  valid  for  all  the  cases. 

b)  For  the  third  quarter,  the  angle  93  is  given  by  the 
following  equation  : 

63=arcsin(  y°"y3  ) 

The  coordinates  of  point  4  are  now  given  by  the  following 
expressions  : 

x4=x3-d34cos02 

y4=y3+Cf34Sin02 

Finally,  the  angle  a  that  enters  the  control  law  appears  as 
below  : 


a=n-e2 


c)  For  the  second  quarter,  93  is  given  by  the  following 
equation  : 
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63=arcsin(  *2  *°  ) 
3  R 


while   x4,    y4   are   given   as 


x4=x3-d34sin82 


y4ssy3-dj4ciose2 


For  this  case,  the  angle  a  appear  as 


a=— *-e, 

2    2 


d)    Similarly,    for   the    case    of   the    first    quarter 


63=arcsin(  y3  y°  ) 
3  R 


x4=x3+cf34cos02 


y4=y3-^34sine2 
o=-e, 
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2.   COUNTERCLOCKWISE  MOTION 

From  Figure  25  it  can  be  seen  that  the  equations 
(4.2),  (4.3),  (4.4),  (4.5),  (4.7),  (4.8),  (4.9),  (4.12), 
(4.13),  (4.14)  continues  to  hold. 

a)  For  the  fourth  quarter,  Figure  25,  angle  63,  x4,  y4  and 
a  appear  as  follows  : 

63=arcsin(  y3"y°} 
3  R 

x4=x3-d34cos02 

O=7l+02 

The  control  law  for  this  and  also  for  the  other  three  cases  of 
the  counterclockwise  motion  appears  as  below  : 

8=ic1sin  (\|f-a-o)  +k2r 

b)  For  the  third  quarter  the  following  relations  hold  : 


e3=arcsin(^^- 


x4=x3+d34sin62 
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Figure  25.  Guidance  Scheme  for  CCW  Motion. 
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y4=y3 -d34cos0: 


2    2 


c)  For  the  second  quarter  similarly  we  have  : 


e3=arcsin(y°  y^ 


R 
x4=x3+d34cos02 

y4=y3+d34sine2 

a=02 

d)  Finally  for  the  first  quarter  the  following  equations  hold 


e3=arcsin(X3"Xo) 


R 
x4=x3-d34sin02 

2    2 


50 


C.   SIMULATION  RESULTS 

By  introducing  the  modifications  of  the  previous  chapter 
into  the  simulation  program,  the  necessity  of  providing  the 
simulation  program  with  a  set  data,  in  order  for  the  vehicle 
to  perform  the  desired  commands,  occurred.  The  table  below 
summarizes  the  required  data,  for  the  simulation  program,  for 
a  test  case  of  a  figure  -  eight  manoeuvre  shown  in  Figure  28. 


X 

y 

TURN 

D 

PATH 

ICCW 

*o 

y0 

15.0 

0.0 

0 

1.0 

0 

0 

0.0 

0.0 

15.0 

10.0 

0 

0.1 

0 

0 

0.0 

0.0 

10.0 

15.0 

0 

0.1 

1 

1 

10.0 

10.0 

8.0 

15.0 

0 

0.1 

0 

0 

0.0 

0.0 

3.0 

20.0 

0 

0.1 

1 

0 

8.0 

20.0 

8.0 

25.0 

0 

0.1 

1 

0 

8.0 

20.0 

13.0 

20.0 

0 

0.1 

1 

0 

8.0 

20.0 

8.0 

15.0 

0 

0.1 

1 

0 

8.0 

20.0 

6.0 

15.0 

0 

0.1 

0 

0 

0.0 

0.0 

1.0 

10.0 

0 

0.1 

1 

1 

6.0 

10.0 

6.0 

5.0 

0 

0.1 

1 

1 

6.0 

10.0 

51 


In  the  first  two  columns  the  coordinates  x,  y  of  the  next  way 
point  are  given.  In  the  third  column,  the  variable  TURN 
selects  how  the  vehicle  is  going  to  change  its  direction  for 
the  next  way  point  in  order  to  have  a  smooth  change  in  course 
with  less  overshoot.  There  are  two  ways,  1  for  circle,  0  for 
distance.  When  1  is  selected  the  vehicle  must  enter  a  circle 
with  a  preselected  target  radius  centered  at  the  way  point 
before  it  starts  heading  for  the  next  one.  When  0  is  selected 
the  vehicle  turns  when  it  reaches  a  preselected  target 
distance  from  the  heading  point.  The  fourth  column,  D, 
determines  this  preselected  radius  or  distance  that  the 
vehicle  has  to  approach  the  way  point  before  it  starts  turning 
for  the  next  one.  The  fifth  column  determines  if  the  desired 
path  is  a  circular  arc  (1)  or  a  straight  line  segment  (0) .  In 
the  sixth  column,  the  (ICCW)  index  determines  if  the  desired 
path  is  in  the  counterclockwise  direction  CCW  for  1,  or 
clockwise  CW  for  0.  This  is  the  case,  of  course,  when  the 
fifth  column  is  1  (circular  path)  .  Finally,  the  two  last 
columns  give  the  coordinates  of  the  center  of  the  circular  arc 
if  this  is  the  case.  In  an  effort  to  make  a  comparison  with 
the  previous  case  of  Figures  22  and  23,  the  vehicle  path  was 
plotted  for  a  two  way  circular  path.  From  Figure  26  can  be 
seen  that  the  reference  path  was  achieved  with  the  use  of  only 
two  points  instead  of  13.  From  Figure  27  we  can  also  see  that 
the  rudder  steers  the  vehicle  in  a  smooth  way  achieving  much 
smaller  values  than  any  other  case  of  straight  line  segments. 
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Figure  26.  Actual  and  Reference  Vehicle  Path. 
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Figure  27.  Rudder  Time  History  for  the  Simulation  of 
Figure  26. 
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After  introducing  the  previous  table  of  data  into  the 
simulation  program,  a  figure  eight  manoeuvre  was  performed  by 
the  vehicle,  Figure  28.  The  rudder  time  history  of  the  above 
manoeuvre  was  also  plotted,  Figure  29,  The  small  values  of  the 
rudder  angle  even  for  a  complicated  manoeuvre  like  this  prove 
the  advantage  of  the  method,  over  the  use  of  line  segments 
only. 
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Figure  28.  Figure  -  Eight  (8)  Manoeuvre. 
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Figure  29.  Rudder  Time  History  for  the  Manoeuvre  of  Figure 
28. 
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CONCLUSIONS  AND  RECOMMENDATIONS 

The  main  conclusions  and  contributions  of  this  work  can  be 
summarized  as  follows  : 

1.  The  use  of  Nomoto's  model  allows  for  a  very  efficient 
steering  control  design  with  excellent  stability  properties 
when  coupled  to  a  pursuit  guidance  scheme. 

2.  An  approximate  stability  condition  was  developed  which 
is  easy  to  compute  and  provides  a  very  good  estimate  of  the 
exact  stability  condition. 

3.  Sensitivity  analysis  of  the  stability  curves  revealed 
the  most  critical  hydrodynamic  coefficients  for  stability. 

4 .  The  use  of  circular  reference  paths  achieves  smooth 
path  response  along  curved  segments  with  very  reasonable 
rudder  activity. 

Finally,  some  recommendations  for  future  research  include 
the  analysis  of  path  accuracy  and  disturbance  rejection 
properties  of  the  scheme,  as  well  as  the  impact  of  sensor  bias 
and  noise. 
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APPENDIX  A 


C      PROGRAM  NOMO.FOR 

C 

C 

C      FOTIS  A.  PAPOULIAS  -  DIMITRIOS  A.  SIMAKIS 

C      NAVAL  POSTGRADUATE  SCHOOL 

C      MONTEREY  CALIFORNIA 

C      JUNE  19  92 

C 

C      CRITICAL  STABILITY  PROGRAM 

C 

C 

REAL  K1,K2,K3,L,NR,NV,NDRS,NDRB, IZ, MASS, 
&      NRDOT,NVDOT 


C 
C 


OPEN  (10,FILE='NOMO.RES' , STATUS  =  'NEW ) 
WEIGHT=435.0 


IZ 

=  45.0 

L 

=  7.3 

RHO 

=  1.94 

G 

=  32.2 

XG 

=0.0104 

MASS 

=WEIGHT/G 

MASS 

=MASS/ (0.5*RHO*L**3) 

IZ 

=IZ/ (0.5*RHO*L**5) 

XG 

=XG/L 

YRDOT 

=-0.00000 

YVDOT 

=-0.03430 

YR 

=+0.00000 

YV 

=-0.10700 

YDRS 

=+0.01241 

YDRB 

=+0.01241 

NRDOT 

=-0.00047 

NVDOT 

=-0.00000 

NR 

=-0.00390 

NV 

=-0.00000 

NDRS 

=-0.337*0.01241 

NDRB   =+0.283*0.01241 

DH   = (IZ-NRDOT) * (MASS-YVDOT) - 
&       (MASS*XG-YRDOT) * (MASS*XG-NVDOT) 

AA11= ( (IZ-NRDOT) *YV- (MASS*XG-YRDOT) *NV) /DH 

AA12= ( (IZ-NRDOT) * (-MASS+YR) - 
&       (MASS*XG-YRDOT) * (-MASS*XG+NR) ) /DH 
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c 


AA21= ( (MASS-YVDOT) *NV- (MASS*XG-NVDOT) *YV) /DH 
AA22= ( (MASS-YVDOT) * (-MASS*XG+NR) - 
&       (MASS*XG-NVDOT) * (-MASS+YR) ) /DH 
BB11=  (  (IZ-NRDOT) *YDRS- (MASS*XG-YRDOT) *NDRS) /DH 
BB12= ( (IZ-NRDOT) *YDRB- (MASS*XG-YRDOT) *NDRB) /DH 
BB21= ( (MASS-YVDOT) *NDRS- (MASS*XG-NVDOT) *YDRS) /DH 
BB22= ( (MASS-YVDOT) *NDRB- (MASS*XG-NVDOT) *YDRB) /DH 

WRITE  (*,1006) 

READ   (*,*)     RATIO 

BB1=BB11+RATI0*BB12 
BB2=BB21+RATIO*BB22 

WRITE  (*,1001) 

READ   (*,*)      ICON 

GO  TO  (100,200) ,  ICON 
100  WRITE  (*, 1002) 

READ   (*,*)     ZMIN,ZMAX, IZETA 

WRITE  (*,1003) 

READ   ( * , * )     WN 

INCR=IZETA 

GO  TO  50 
200  WRITE  (*, 1004) 

READ   (*,*)     WNMIN,WNMAX, IWN 

WRITE  (*,1005) 

READ   (*,*)     ZETA 

INCR=IWN 

50  CI- (AA11*AA22-AA21*AA12) * (AA21*BB1-AA11*BB2) 
C2= (AA11+AA22) * (AA21*BB1-AA11*BB2) 
&+BB2* (AA11*AA22-AA21*AA12) 
C3=- (AA21*BB1-AA11*BB2)  **2 
A=C1/C2 
B=C3/C2 

DO  1  1=1, INCR 

IF  (ICON.EQ.l)  ZETA=ZMIN  + (ZMAX  -ZMIN  )  *  (1-1) /  (INCR-1) 
IF  (ICON.EQ.2)  WN  =WNMIN+ (WNMAX-WNMIN) * (1-1 )/ (INCR-1) 

IF  (ICON.EQ.l)  OUT=ZETA 
IF  (ICON.EQ.2)  OUT=WN 

K1=-WN*WN/B 

K2—  (A+2  .  0*ZETA*WN)  /B 

B1P—  (AA11+AA22)  -BB2*K2 

C1P= (AA11*AA22-AA12*AA21) + (BB2*AA1 1-BB1 *AA21 ) 
&   *K2-BB2*K1 
C2P=-BB1*K1 
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D1P= (AA11*BB2-AA21*BB1) *K1 

D2P= (BB1*AA22-BB2*AA12) *K1-BB2*K1 

E2P= (AA11*BB2-AA21*BB1) *K1 

AQ=B1P*C1P*D1P-D1P*D1P 

BQ=B1P*C1P*D2P+B1P*C2P*D1P-B1P*B1P*E2P-2.0*D1P*D2P 

CQ=B1P*C2P*D2P-D2P*D2P 

DET=BQ*BQ-4 . 0*AQ*CQ 

IF  (DET.LT.O.O)  GO  TO  1 

XD1= (-BQ+SQRT (DET) ) / (2 . 0*AQ) 

XD2=  (-BQ-SQRT (DET) ) / (2 . 0*AQ) 

C0EF1=BB1*AA22-BB2*AA12-BB2 
COEF2=BB2  *AA1 1 -BB1 *  AA2 1 

VAL1=1.0+COEF1/ (XDl*COEF2) 
VAL2=1.0+COEF1/ (XD2*COEF2) 

IF  ( (VAL1.LT.0.0) .OR. (XD1.LT.0.0) )  XD1=0 . 0 
IF  ( (VAL2.LT. 0.0) .OR. (XD2.LT. 0.0) )  XD2=0 . 0 
XD3=1 .0/(2 . 0*ZETA*WN) 

WRITE  (10,2001)  XD1,XD2,XD3,0UT,K1,K2 
1  CONTINUE 


1001  FORMAT 
& 

1002  FORMAT 

1003  FORMAT 

1004  FORMAT 

1005  FORMAT 
100  6  FORMAT 
2001  FORMAT 

END 


('  ENTER  1  :  ZETA  VARIATION' , /, 
'         2  :  WN    VARIATION' ) 

MIN,MAX,  AND  INCREMENTS  OF  ZETA') 

WN'  ) 

MIN,MAX,  AND  INCREMENTS  OF  WN' ) 

ZETA' ) 

BOW/STERN  RUDDER  RATIO' ) 


(' 
(' 
(' 
(' 
(' 


ENTER 
ENTER 
ENTER 
ENTER 
ENTER 


(6E15.5) 
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APPENDIX  B 


C      PROGRAM  NOMOV.FOR 

C 

C 

C      FOTIS  A.  PAPOULIAS  -  DIMITRIOS  A.  SIMAKIS 

C      NAVAL  POSTGRADUATE  SCHOOL 

C      MONTEREY  CALIFORNIA 

C      JUNE  1992 

C 

C 

C      SENSITIVITY  OF  HYDRODYNAMIC  COEFFICIENTS  PROGRAM 

C 

C 

REAL  K1,K2,K3,L,NR,NV,NDRS,NDRB, I Z, MASS, 
&      NRDOT,NVDOT 


C 
C 


OPEN  (10,FILE='NOMOV.RES' , STATUS=' NEW' ) 

WEIGHT=435.0 

IZ  =45.0 

L  =7.3 

RHO  =1.94 

G  =32.2 

XG  =0.0104 

MASS  =WEIGHT/G 

MASS  =MASS/ (0.5*RHO*L**3) 

IZ  =IZ/  (0.5*RHO*L**5) 

XG  =XG/L 

YRDOT  =-0.00000 

YVDOT  =-0.03430 

YR  =+0.00000 

YV  =-0.10700 

YDRS  =+0.01241 

YDRB  =+0.01241 

NRDOT  =-0.00047 

NVDOT  =-0.00000 

NR  =-0.00390 

NV  =-0.00000 

NDRS  =-0.337*0.01241 

NDRB  =+0.283*0.01241 

DH   = (IZ-NRDOT) * (MASS-YVDOT)- 
&       (MASS*XG-YRDOT) * (MASS*XG-NVDOT) 
AA11= ( (IZ-NRDOT) *YV- (MASS*XG-YRDOT) *NV) /DH 
AA12= ( (IZ-NRDOT) * (-MASS+YR) - 


62 


&       (MASS*XG-YRDOT) * (-MASS*XG+NR) ) /DH 
AA21= ( (MASS-YVDOT) *NV- (MASS*XG-NVDOT) *YV) /DH 
AA22= ( (MASS-YVDOT) * (-MASS*XG+NR) - 

&       (MASS*XG-NVDOT) * (-MASS+YR) ) /DH 
BB11= ( (IZ-NRDOT) *YDRS- (MASS*XG-YRDOT) *NDRS) /DH 
BB12= ( (IZ-NRDOT) *YDRB- (MASS*XG-YRDOT) *NDRB) /DH 
BB21= ( (MASS-YVDOT) *NDRS- (MASS*XG-NVDOT) *YDRS) /DH 
BB22= ( (MASS-YVDOT) *NDRB- (MASS*XG-NVDOT) *YDRB) /DH 

RATIO=-l .0 

BB1=BB11+RATI0*BB12 
BB2=BB21+RATIO*BB22 

ZETA=0.8 
WN=3.0 

Cl  = (AA11*AA22-AA21*AA12) * (AA21*BB1-AA11 *BB2 ) 
C2= (AA11+AA22) * (AA21 *BB1-AA11*BB2 ) +BB2 
&    * (AA11*AA22-AA21*AA12) 
C3=- (AA21*BB1-AA11*BB2) **2 
A=C1/C2 
B=C3/C2 

K1=-WN*WN/B 

K2=- (A+2 . 0*ZETA*WN) /B 

B1P=- (AA11+AA22) -BB2*K2 

C1P= (AA11*AA22-AA12*AA21) + (BB2*AA11-BB1*AA21) 
&       *K2-BB2*K1 
C2P=-BB1*K1 

D1P= (AA11*BB2-AA21*BB1) *K1 
D2P= (BB1*AA22-BB2*AA12) *K1-BB2*K1 
E2P= (AA11*BB2-AA21*BB1) *K1 

AQ=B1P*C1P*D1P-D1P*D1P 

BQ=B1P*C1P*D2P+B1P*C2P*D1P-B1P*B1P*E2P-2.0*D1P*D2P 

CQ=B1P*C2P*D2P-D2P*D2P 

DET=BQ*BQ-4 . 0*AQ*CQ 

IF  (DET.LT.0.0)  GO  TO  1 

XDNOM= (-BQ+SQRT (DET) ) / (2 . 0*AQ) 

HOLD=YVDOT 

DO  1  1=1,101 

VARY=0.5+  (1-1) /100.0 
YVDOT=0.5*HOLD+HOLD*  (1-1) /100.0 

DH   = (IZ-NRDOT) * (MASS-YVDOT) - 
&       (MASS*XG-YRDOT) * (MASS*XG~NVDOT) 
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AA11= ( (IZ-NRDOT) *YV- (MASS*XG~YRDOT) *NV) /DH 

AA12= ( (IZ-NRDOT) * (-MASS+YR) - 
&       (MASS*XG-YRDOT) * (-MASS*XG+NR) ) /DH 

AA21= ( (MASS-YVDOT) *NV- (MASS*XG-NVDOT) *YV) /DH 

AA22= ( (MASS-YVDOT) * (-MASS*XG+NR) - 
&       (MASS*XG-NVDOT) * (-MASS+YR) ) /DH 

BB11= ( (IZ-NRDOT) *YDRS~ (MASS*XG-YRDOT) *NDRS) /DH 

BB12=  (  (IZ-NRDOT) *YDRB- (MASS*XG-YRDOT) *NDRB) /DH 

BB21=  (  (MASS-YVDOT) *NDRS- (MASS*XG-NVDOT) *YDRS) /DH 

BB22= ( (MASS-YVDOT) *NDRB- (MASS*XG-NVDOT) *YDRB) /DH 

BB1=BB11+RATI0*BB12 

BB2=BB21+RATIO*BB22 

B1P=- (AA11+AA22) -BB2*K2 

C1P= (AA11*AA22-AA12*AA21) + (BB2*AA11-BB1 *AA21 ) 
&    *K2-BB2*K1 
C2P=-BB1*K1 

D1P= (AA11*BB2-AA21*BB1) *K1 
D2P= (BB1*AA22-BB2*AA12) *K1-BB2*K1 
E2P= (AA11*BB2-AA21*BB1) *K1 

AQ=B1P*C1P*D1P-D1P*D1P 

BQ=BlP*ClP*D2P+BlP*C2P*DlP-BlP*BlP*E2P-2 . 0*D1P*D2P 

CQ=B1P*C2P*D2P-D2P*D2P 

DET=BQ*BQ-4 . 0*AQ*CQ 

IF  (DET.LT.0.0)  GO  TO  1 

XD1=(-BQ+SQRT(DET) ) / (2.0*AQ) 

XD2= (-BQ-SQRT (DET) ) / (2 . 0*AQ) 

C0EF1=BB1*AA22-BB2*AA12-BB2 
COEF2=BB2  *AA1 1-BB1 *AA2 1 

VAL1=1.0+COEF1/ (XDl*COEF2) 
VAL2=1.0+COEF1/ (XD2*COEF2) 

IF  ( (VAL1.LT.0.0) .OR. (XD1.LT.0.0) )  XD1=0 . 0 
IF  ( (VAL2.LT. 0.0) .OR. (XD2.LT. 0.0) )  XD2=0 . 0 
XD3=1 .0/(2 . 0*ZETA*WN) 
WRITE  (10,2001)  XDl/XDNOM,VARY 
1  CONTINUE 


1001  FORMAT  ('  ENTER  1  :  ZETA  VARIATION',/, 


10 

10 
10 
10 
10 
20 


02  FORMAT 

03  FORMAT 

04  FORMAT 

05  FORMAT 

0  6  FORMAT 

01  FORMAT 
END 


'        2  :  WN    VARIATION') 

'  ENTER  MIN,MAX,  AND  INCREMENTS  OF  ZETA') 

'  ENTER  WN' ) 

'  ENTER  MIN,MAX,  AND  INCREMENTS  OF  WN' ) 

'  ENTER  ZETA' ) 

'  ENTER  BOW/STERN  RUDDER  RATIO' ) 

2E15.5) 
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APPENDIX  C 


C      PROGRAM  SIMU.FOR 

C 

C 

C      FOTIS  A.  PAPOULIAS  -  DIMITRIOS  A.  SIMAKIS 

C      NAVAL  POSTGRADUATE  SCHOOL 

C      MONTEREY  CALIFORNIA 

C      JUNE  1992 

C 

C      SIMULATION  PROGRAM  FOR  CONTROL  AND  GUIDANCE 

C      ALONG  STRAIGHT  LINES  AND 

C      CIRCULAR  TRAJECTORIES 

C 

C 

REAL  K1,K2,L,NR,NV, NDRS,NDRB, I Z, MASS, 
&      NRDOT,NVDOT 

C 

OPEN     (10,FILE=' SIMU.RES' , STATUS='NEW ) 
OPEN     (11,FILE=' SIMU.DAT' , STATUS=' OLD' ) 


c 

c 

GEOMETRIC  PROPERTIES 

c 

WEIGHT=435.0 

IZ 

=  45.0 

L 

=7.3 

RHO 

=  1.94 

G 

=32.2 

XG 

=0.0104 

MASS 

=WEIGHT/G 

MASS 

=MASS/ (0.5*RHO*L**3 

IZ 

=IZ/ (0.5*RHO*L**5) 

XG 

=XG/L 

PI 

=3.1415926536 

c 

c 

HYDRODYNAMIC  COEFFICIENTS 

c 

YRDOT 

=-0.00000 

YVDOT 

=-0.03430 

YR 

=+0.00000 

YV 

=-0.10700 

YDRS 

=+0.01241 

YDRB 

=+0.01241 

NRDOT 

=-0.00047 

NVDOT 

=-0.00000 

NR 

=-0.00390 

NV 

=-0.00000 
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c 
c 


NDRS   =-0.337*0.01241 
NDRB   =+0.283*0.01241 


DH   = 

AA11  = 
AA12  = 

K 

AA21  = 
AA22  = 
■ 

BB11  = 
BB12  = 
BB21  = 
BB22  = 


IZ-NRDOT) * (MASS-YVDOT) - 
MASS*XG-YRDOT) * (MASS*XG-NVDOT) 
(IZ-NRDOT) *YV- (MASS*XG-YRDOT) *NV) /DH 
(IZ-NRDOT) * (-MASS+YR) - 
MASS*XG-YRDOT) * (-MASS*XG+NR) ) /DH 
(MASS-YVDOT) *NV- (MASS*XG-NVDOT) *YV) /DH 
(MASS-YVDOT) * (-MASS*XG+NR) - 
MASS*XG-NVDOT) * (-MASS+YR) ) /DH 
(IZ-NRDOT) *YDRS- (MASS*XG~YRDOT) *NDRS) /DH 
(IZ-NRDOT) *YDRB- (MASS*XG-YRDOT) *NDRB) /DH 
(MASS-YVDOT) *NDRS- (MASS*XG-NVDOT) *YDRS) /DH 
(MASS-YVDOT) *NDRB- (MASS*XG-NVDOT) *YDRB) /DH 


RATIO=-l 

BBl=BBll+RATIO*BB12 
BB2=BB21+RATIO*BB22 


ZETA=0 . 8 

WN=3.0 

DIST=1.0 

C1=(AA11*AA22-AA21*AA12) * (AA21*BB1-AA11*BB2 ) 
C2= (AA11+AA22) * (AA21*BB1-AA11*BB2) +BB2 
&     * (AA11*AA22-AA21*AA12) 
C3=- (AA21*BB1-AA11*BB2) **2 
A=C1/C2 
B=C3/C2 

K1=-WN*WN/B 

K2=- (A+2 . 0*ZETA*WN) /B 


C 
C 
C 


STIME=150.0 

DELTAT=0.1 

ITIME=STIME/DELTAT 

INITIAL  CONDITIONS 


PSI  = 
V=0. 
R=0. 
Y=0. 
X=0. 
X1  =  0 
Y1  =  0 
J=0 


0.0 

0 

0 

0 

0 

.0 

.0 


PRINT*, 'ENTER  NUMBER  OF  POINTS' 
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c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 

c 
c 
c 


READ*,NUM 
THE  SIMULATION  STARTS  FROM  THE  POINT  (0.0,0.0)  AND  THE  NEXT 
POINT  (X2,Y2)  IS  PROVIDED  BY  THE  DATA  FILE. 


ITURN    DETERMINES  THE  WAY  THE  VEHICLE  IS 

TO  TURN  TO  PROCEED  FOR  THE  NEXT  POINT 


ITURN  1  FOR  CIRCLE 
ITURN  0  FOR  DISTANCE 


DD 


DETERMINES  THE  DISTANCE  OF  THE  VEHICLE 
FROM  XZ,Y2  WHEN  THE  TURN  STARTS. 


IPATH    DETERMINES  IF  THE  DESIRE  PATH  TO  X2,Y2 
IS  A  CIRCULAR  ONE  OR  A  STRAIGHT  LINE. 

IPATH  1  FOR  CIRCULAR 

IPATH  0  FOR  STRAIGHT  LINE 

ICCW   0  FOR  CW 
1  FOR  CCW 

X0,Y0   IS  THE  DESIRED  CENTER  OF  THE  CIRCULAR  PATH 
AND  X0,Y0  ARE  PROVIDED  FROM  THE  DATA  FILE 
AND  IF  NOT  THE  DATA 
FILE  PROVIDES  ZERO  VALUES. 

READ(11,  *)  X2,  Y2,  ITURN, DD,  IPATH,  ICCW,X0,  Y0 

SIMULATION  STARTS 

DO  1  I=1,ITIME 
TIME=I*DELTAT 


X21=X2-X1 

Y21=Y2-Y1 

IF  (  (X21.EQ.0.)  .AND.  (Y21.GT.0.)  )  ANA=+0 

IF  (  (X21.EQ.0.)  .AND.  (Y21.LT. 0.)  )  ANA=-0 

IF  (X21.EQ.0.)  GO  TO  93 

ANA=ATAN(ABS (Y21) /ABS (X21) ) 

IF  ( (X21.GE.0.) .AND. (Y21.GE.0.) )  ANA= 


93 


IF  (  (X21.GE.0.)  .AND 
IF  (  (X21.LT.0.)  .AND 
IF  ( (X21.LT.0.) .AND 
CONTINUE 


5*PI 
5*PI 


ANA 
-ANA 


(Y21.LT. 0.))  ANA 
(Y21.GE.0.))  ANA=  PI-ANA 
(Y21.LT.0.))  ANA=-PI+ANA 
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C       EQUATIONS  OF  MOTION 
C 

PSIDOT=R 

VD0T=AA11*V+AA12*R+BB1*DR 

RDOT=AA2 1 *V+AA22  *R+BB2  *DR 

YDOT=SIN (PSI) +V*COS (PSI) 

XDOT=COS (PSI)-V*SIN(PSI) 
C 

C       FIRST  ORDER  INTEGRATION 
C 

PSI=PSI+DELTAT*PSIDOT 

V=V+DELTAT * VDOT 

R=R+DELTAT*RDOT 

Y=Y+DELTAT*YDOT 

X=X+DELTAT*XDOT 


C 
C 


c 
c 


IF  (PSI.GT.  (2.0*PI) )  PSI=PSI-2.0*PI 

YPR=(Y-Y1) *COS (ANA)- (X-Xl) *SIN(ANA) 

PSIC=-ATAN (YPR/DIST) 

IF  (IPATH.EQ.O)  DR=K1*SIN( (PSI-PSIC-ANA) ) +K2*R 


IF  (IPATH.EQ.l)  THEN 

DALF1= ( (X-XO) * (X-XO)  +  (Y-YO) * (Y-YO)  )  **0  .5 
RADIUS=(  (X1-X0) *  (X1-X0)  +  (Y1-Y0) * (Y1-Y0)  )  **0 
X3=X0+RADIUS/DALF1* (X-XO) 
Y3=Y0+RADIUS/DALF1* (Y-YO) 
THONE=PI/2.-DIST/ (2.*RADIUS) 
C       WRITE  (*,*)  X,  Y,X3,  Y3,X0,  YO/RADIUS 
C 

IF  (ICCW.EQ.O)  THEN 
C 

IF  (  (X21.GT.0.0)  .AND.  (Y21.GT.0.0) )  THEN 

THTHR=ASIN (ABS (X0-X3) /RADIUS) 

THTWO=PI-THONE-THTHR 

DISTF=2.0*RADIUS*SIN(DIST/ (2.0*RADIUS) ) 

X4=X3+DISTF*SIN (THTWO) 

Y4=Y3+DISTF*COS (THTWO) 

DALF2=( (X-X4) * (X-X4) + (Y-Y4) * (Y-Y4) ) **0.5 

BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 

IF  (BALFA.GT.1.0)  BALFA=1.0 

IF  (BALFA.LE.1.0)  AALFA=ASIN (BALFA) 

SIGMA=AALFA-THONE 

THTWO=0 . 5*PI-THTWO 

END  IF 
C 

IF  (  (X21.GT. 0.0)  .AND.  (Y21.LT. 0.0) )  THEN 

THTHR=ASIN (ABS (Y0-Y3) /RADIUS) 

THTWO=PI-THONE-THTHR 

DISTF=2.0*RADIUS*SIN(DIST/ (2.0*RADIUS) ) 
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c 
c 


X4=X3+DISTF*COS (THTWO) 

Y4=Y3-DISTF*SIN (THTWO) 

DALF2= ( (X-X4) * (X-X4)  +  (Y-Y4) * (Y-Y4) ) **0  .5 

BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 

IF  (BALFA.GT.1.0)  BALFA=1.0 

IF  (BALFA.LE.1.0)  AALFA=ASIN (BALFA) 

S I GMA=AALFA-THONE 

THTWO=-THTWO 

END  IF 

IF  (  (X21.LT. 0.0)  .AND.  (Y21.LT. 0.0) )  THEN 

THTHR=ASIN (ABS (X0-X3) /RADIUS) 

THTWO=PI-THONE-THTHR 

DISTF=2.0*RADIUS*SIN(DIST/ (2.0*RADIUS) ) 

X4=X3-DISTF*SIN (THTWO) 

Y4=Y3-DISTF*COS (THTWO) 

DALF2=( (X-X4) * (X-X4)+ (Y-Y4) * (Y-Y4) ) **0.5 

BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 

IF  (BALFA.GT.1.0)  BALFA=1.0 

IF  (BALFA.LE.1.0)  AALFA=AS IN (BALFA) 

SIGMA=AALFA-THONE 

THTWO=l . 5*PI-THTWO 

END  IF 

IF  ( (X21.LT. 0.0) .AND. (Y21.GT. 0.0) )  THEN 

THTHR=ASIN (ABS (Y0-Y3) /RADIUS) 

THTWO=PI-THONE-THTHR 

DISTF=2.0*RADIUS*SIN(DIST/ (2.0*RADIUS) ) 

X4=X3-DISTF*COS (THTWO) 

Y4=Y3+DISTF*SIN (THTWO) 

DALF2= ( (X-X4) * (X-X4) + (Y-Y4) * (Y-Y4) ) **0 .5 

BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 

IF  (BALFA.GT.1.0)  BALFA=1 . 0 

IF  (BALFA.LE.1.0)  AALFA=AS IN (BALFA) 

SIGMA=AALFA-THONE 

THTWO=PI-THTWO 

END  IF 


END  IF 

IF  (ICCW.EQ.l)  THEN 

IF  ( (X21.GT.0.0)  .AND.  (Y21.GT.0.0) )  THEN 

THTHR=ASIN (ABS (Y0-Y3) /RADIUS) 

THTWO=P I -THONE-THTHR 

DISTF=2 . 0*RADIUS*SIN (DIST/ (2 . 0*RADIUS) ) 

X4=X3+DISTF*COS (THTWO) 

Y4=Y3+DISTF*SIN (THTWO) 

DALF2= ( (X-X4) * (X-X4) + (Y-Y4) * (Y-Y4) ) **0.5 

BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 
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c 
c 


IF  (BALFA.GT.1.0)  BALFA=1.0 

IF  (BALFA.LE.1.0)  AALFA=ASIN (BALFA) 

S I GMA=AALFA-THONE 

THTWO=THTWO 

END  IF 

IF  (  (X21.GT. 0.0)  .AND.  (Y21.LT. 0.0) )  THEN 

THTHR=ASIN (ABS (X0-X3) /RADIUS) 

THTWO=PI-THONE-THTHR 

DISTF=2.0*RADIUS*SIN(DIST/ (2.0*RADIUS) ) 

X4=X3+DISTF*SIN (THTWO) 

Y4=Y3-DISTF*COS (THTWO) 

DALF2=( (X-X4) * (X-X4).+ (Y-Y4) * (Y-Y4) ) **0 .! 

BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 

IF  (BALFA.GT.1.0)  BALFA=1 . 0 

IF  (BALFA.LE.1.0)  AALFA=AS IN (BALFA) 

SIGMA=AALFA-THONE 

THTWO=l .  5*PI  +  THTWO 

END  IF 

IF  (  (X21.LT. 0.0)  .AND.  (Y21.LT. 0.0) )  THEN 

THTHR=ASIN (ABS (Y0-Y3) /RADIUS) 

THTWO=PI-THONE-THTHR 

DISTF=2.0*RADIUS*SIN(DIST/ (2.0*RADIUS) ) 

X4=X3-DISTF*COS (THTWO) 

Y4=Y3-DISTF*SIN (THTWO) 

DALF2=( (X-X4) * (X-X4) + (Y-Y4) * (Y-Y4) )  **0.! 

BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 

IF  (BALFA.GT.1.0)  BALFA=1.0 

IF  (BALFA.LE.1.0)  AALFA=AS IN (BALFA) 

SIGMA=AALFA-THONE 

THTWO=PI+THTWO 

END  IF 

IF  (  (X21.LT. 0.0)  .AND.  (Y21 . GT . 0.0)  )  THEN 

THTHR=ASIN (ABS (X0-X3) /RADIUS) 

THTWO=PI-THONE-THTHR 

DISTF=2.0*RADIUS*SIN(DIST/ (2.0*RADIUS) ) 

X4=X3-DISTF*SIN (THTWO) 

Y4=Y3+DISTF*COS (THTWO) 

DALF2= ( (X-X4) * (X-X4)  +  (Y-Y4) * (Y-Y4)  ) **0  .! 

BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 

IF  (BALFA.GT.1.0)  BALFA=1.0 

IF  (BALFA.LE.1.0)  AALFA=AS IN (BALFA) 

S IGMA=AALFA-THONE 

THTWO=0 . 5*PI+THTWO 

END  IF 


END  IF 
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c 
c 

c 
c 


IF  (ICCW.EQ.O)   DR=K1*SIN(PSI-THTW0+SIGMA) +K2*R 
IF  (ICCW.EQ.l)   DR=K1*SIN(PSI-THTW0-SIGMA) +K2*R 

END  IF 

IF  (DR.GT.0.4)   DR=0.4 
IF  (DR.LT.-0.4)  DR=-0.4 

WRITE (10, 2001)  TIME,X, Y,PSI,PSIC,DR, YPR 

TURN= (X-X2) * (X-X2)  +  (Y-Y2) *  (Y-Y2) 

TURNC=SQRT(TURN) 
TURNL=SQRT (TURN-YPR*YPR) 
IF  (ITURN.EQ.l)  TD=TURNL 
IF  (ITURN.EQ.O)  TD=TURNC 
IF  (TD.LT.DD)  THEN 
8        J=J+1 

IF  (J.EQ.NUM)  GO  TO  2002 
IF  (J.NE.NUM)  THEN 
X1=X2 
Y1=Y2 

READ (11, *)  X2, Y2, ITURN,DD, IPATH, ICCW,X0,  Y0 
GO  TO  1 
END  IF 
END  IF 
C 

1      CONTINUE 

2001  FORMAT  (7E15.5) 

2002  STOP 
END 


C      PROGRAM  REFERENCE  PATH 

C 

C      DIMITRIOS  SIMAKIS 

C      NAVAL  POSTGRADUATE  SCHOOL 

C      MONTEREY,  CALIFORNIA 

C      JUNE  1992 

C 

OPEN  (11,FILE='SIMUR.REF' , STATUS=' OLD' ) 

OPEN  (10,FILE='SIMUR.RES' , STATUS-' NEW ) 

X1=0.0 

Y1  =  0.0 

PI=3.141 

PRINT*, 'ENTER  NUMBER  OF  POINTS' 

READ*,NUM 
C 

DO  1  I=1,NUM+1 
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IF  (I.EQ.l)  THEN 

XR=X1 

YR=Y1 

WRITE  (10,100)  XR,YR 

GO  TO  1 

END  IF 

READ  (11,*)  X2, Y2, ITURN,DD, IPATH, ICCW,X0, Y0 

THETA=0.0 

RADIUS=( (X1-X0) * (X1-X0)+ (Y1-Y0) * (Y1-Y0) ) **0.5 

X21=X2-X1 

Y21=Y2-Y1 

DO  2  J=l/200 

THETA=  THETA+  0.01745 

IF  (IPATH.EQ.l)  THEN 

IF  (ICCW.EQ.O)  THEN 

IF  (X21.GT.0.0.AND.Y21.GT.0.0)  THEN 
XR=X0-RADIUS*COS (THETA) 
YR=YO+RADIUS*SIN (THETA) 
END  IF 


IF  (X21.GT. 0.0. AND. Y21.LT. 0.0) 
XR=XO+RADIUS*SIN (THETA) 
YR=Y0+RADIUS*COS (THETA) 

END  IF 


THEN 


IF  (X21.LT. 0.0. AND. Y21 .GT.0.0) 
XR=XO-RADIUS*SIN (THETA) 
YR=Y0-RADIUS*COS (THETA) 

END  IF 


THEN 


IF  (X21 .LT.0.0.AND.Y21 .LT.0.0) 
XR=X0+RADIUS*COS (THETA) 
YR=Y0-RADIUS*SIN (THETA) 
END  IF 
END  IF 


THEN 


IF  (ICCW.EQ.l)  THEN 

IF  (X21.GT.0.0.AND.Y21.GT.0.0)  THEN 
XR=X0+RADIUS*SIN (THETA) 
YR=Y0-RADIUS*COS (THETA) 
END  IF 


IF  (X21.GT. 0.0. AND. Y21.LT. 0.0) 
XR=X0-RADIUS*COS (THETA) 
YR=Y0-RADIUS*SIN (THETA) 

END  IF 


THEN 


IF  (X21.LT. 0.0. AND. Y21 .GT.0.0) 
XR=X0+RADIUS*COS (THETA) 
YR=Y0+RADIUS*SIN (THETA) 

END  IF 


THEN 
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IF     (X21.LT. 0.0. AND. Y21.LT. 0.0)        THEN 
XR=XO-RADIUS*SIN (THETA) 
YR=Y0+RADIUS*COS (THETA) 
END    IF 
END    IF 
END    IF 

i 

IF     (IPATH.EQ.O)     THEN 
XR=X2 
YR=Y2 
END    IF 

■ 

WRITE     (10,100)     XR,YR 

DIS= ( (XR-X2) * (XR-X2)  +  (YR-Y2) * (YR-Y2)  )  **0  .5 
IF     (DIS.LT.0.1)     THEN 
X1=X2 
Y1=Y2 
GO    TO    1 
END    IF 
2  CONTINUE 

100  FORMAT     (2E15.5) 

1  CONTINUE 

STOP 
END 
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